package crtl;

import java.util.HashMap;
import java.util.Map;

/**
 *
 * @author marcel meraner
 */
public class BangBangBalanceBallCtrl extends BangBangCtrl {
	
	private final int _minDSensorValue = 100;
	
	private boolean _finished = false;
	
	public BangBangBalanceBallCtrl() {
		super();
		System.out.println("BangBang - Balance Ball Controller initialized");
	}
	
	@Override
	protected boolean finished() {
		return _finished;
	}

	@Override
	protected Speed navigate() {
		double[] s = readDistanceSensors();
		s = calcAverage(s);
		
		double max = Double.MIN_VALUE;
		Direction d = Direction.FORWARD;
		HashMap<Direction, Double> values = convertSensorValues(s);
		
		// Find highest sensor value and set drive direction to it
		for (Map.Entry<Direction, Double> entry : values.entrySet()) {
			if (entry.getValue() > max) {
				d = entry.getKey();
				max = entry.getValue();
			}
		}
		
		if (max < _minDSensorValue) {
			// Nothing found, drive forward
			d = Direction.FORWARD;
		}
		
		return calcSpeed(d, 1);
	}
	
}
